#include <motor_RS485.hpp>
#include <iostream>

using namespace std;

int main()
{
    cout << "Multi Motor Test Start" << endl;

    modbus_t *modBusCtx = Motor_RS485::CreatModBusInstance("/dev/ttyUSB0", 115200, false);
    if (modBusCtx == nullptr)
    {
        cout << "Creat modbus fail" << endl;
        return -1;
    }

    Motor_RS485 motor1, motor2;
    MotorConfig cfg1{.address = 1, .poleCount = 15};
    MotorConfig cfg2{.address = 2, .poleCount = 15};
    if (!motor1.InitDevice(modBusCtx, cfg1))
    {
        cout << "motor1 init fail" << endl;
        return -1;
    }
    if (!motor2.InitDevice(modBusCtx, cfg2))
    {
        cout << "motor1 init fail" << endl;
        return -1;
    }

    while (true)
    {
        motor1.Select();
        motor1.SendHeartBeat();
        float rpm1 = motor1.GetRPM();

        motor2.Select();
        motor2.SendHeartBeat();
        float rpm2 = motor2.GetRPM();
        cout << "rpm1:" << rpm1 << " rpm2:" << rpm2 << endl;
    }

    motor1.Select();
    motor1.SetControlMode(ControlMode::Speed);
    motor2.Select();
    motor2.SetControlMode(ControlMode::Speed);
    while (true)
    {
        float rpm1, rpm2;
        motor1.Select();
        motor1.SendHeartBeat();
        motor1.SetRPM(50);
        rpm1 = motor1.GetRPM();

        motor2.Select();
        motor2.SendHeartBeat();
        motor2.SetRPM(50);
        rpm2 = motor2.GetRPM();
        cout << "rpm1:" << rpm1 << " rpm2:" << rpm2 << endl;
    }

    return 0;
}
